#ifndef PIDCONTROLLER_HPP
#define PIDCONTROLLER_HPP
#include "Algorithm/PIDController.h"
#include <matplot/matplot.h>


namespace MM {
    using std::tie;

    template<bool Plotting>
    float &PIDInstance::calculate(const float measure, const float ref,
                                  optional<TimePoint> &&tp) noexcept {
        const auto now = tp.value_or(steady_clock::now());
        const auto dt = std::chrono::duration<float, std::milli>(now - last_time).count();
        last_time = now;
        Measure = measure;
        Ref = ref;
        Err = ref - measure;
        if (abs(Err) > DeadBand) {
            Pout = Kp * Err;
            ITerm = Ki * Err * dt;
            Dout = Kd * (Err - Last_Err) / dt;
            //TODO:Improvements...
            integral_limit();
            //
            Iout += ITerm;
            Output = Pout + Iout + Dout;
            Output = clamp(Output, -MaxOut, MaxOut);
        } else {
            Output = 0;
            ITerm = 0;
        }

        tie(Last_Measure, Last_Output, Last_Dout, Last_Err, Last_ITerm) = tie(Measure, Output, Dout, Err, ITerm);

        if constexpr (Plotting) {
            plot_buffer_y.push_back(Output);
            plot_draw_counter++;
            if (plot_draw_counter == 500) {
                const auto start = plot_buffer_y.size() > 2000 ? plot_buffer_y.end() - 2000 : plot_buffer_y.begin();
                const std::vector<float> last_1000_points(start, plot_buffer_y.end());
                matplot::plot(last_1000_points);
                plot_draw_counter = 0;
            }
        }

        return Output;
    }

    void PIDInstance::integral_limit() noexcept {
        const float temp_Iout = Iout + ITerm;
        if (const float temp_Output = Pout + Iout + Dout; abs(temp_Output) > MaxOut) {
            if (Err * Iout > 0) {
                ITerm = 0;
            }
        }
        if (temp_Iout > IntegralLimit) {
            ITerm = 0;
            Iout = IntegralLimit;
        }
        if (temp_Iout < -IntegralLimit) {
            ITerm = 0;
            Iout = -IntegralLimit;
        }
    }

    template float &PIDInstance::calculate<false>(const float measure, const float ref,
                                                  optional<TimePoint> &&tp) noexcept;

    template float &PIDInstance::calculate<true>(const float measure, const float ref,
                                                 optional<TimePoint> &&tp) noexcept;
}
#endif //PIDCONTROLLER_HPP
